
#include "hairmath.h"
#include <vector>

namespace HairEngine
{
	class Link
	{
	public:
		float mass;			//Mass of the link
		Vector3D pos;		//Position of the link
		Vector3D vel;		//Velocity of the link
		Vector3D force;		//Force applied to this link at an instance
	
		//Constructor
		Link(float mass)
		{
			this->mass = mass;
		}
	
		/*
		  void applyForce(Vector3D force) method is used to add external force to the link.
		  At an instance in time, several sources of force might affect the mass. The vector
		  sum of these forces make up the net force applied to the mass at the instance.
		*/
		void applyForce(Vector3D force)
		{
			this->force += force;
		}
		
		/*
		  void init() method sets the individual force components to zero.
		*/
		void init()
		{
			force.x = 0;
			force.y = 0;
			force.z = 0;
		}
	
		/*
		  void simulate(float dt) method calculates the new velocity and new position of 
		  the mass according to change in time (dt). The Euler Method is used. It is not
		  always accurate, but it is simple.
		*/
		void simulate(float dt)
		{
			vel += (force/mass) * dt;		//Change in velocity is added to the velocity. This
											//change is proportional with the acceleration (force/m)
											//and change in time
		
			pos += vel * dt;				//Change in position is added to the position which
											//is velocity times the change in time
		}
	};

	class Spring
	{
	public:
		Link* link1;
		Link* link2;
	
		float springConstant;
		float springLength;
		float frictionConstant;		//TODO: change to dampeningConstant
	
		//Constructor
		Spring(Link* link1, Link* link2, float springConstant, float springLength, float frictionConstant)
		{
			this->springConstant = springConstant;
			this->springLength = springLength;
			this->frictionConstant= frictionConstant;
		
			this->link1 = link1;
			this->link2 = link2;
		}
	
		/*
		  void solve() method where forces can be applied.
		*/
		void solve()
		{
			Vector3D springVector = link1->pos - link2->pos;						//Vector between 2 links
		
			float r = springVector.length();										//The distance between the 2 links
		
			Vector3D force;
		
			if(r != 0)																//Check if r is zero to avoid division by zero
				force += (springVector / r) * (r - springLength) * (-springConstant);	//The spring force is added to the force
		
			force += -(link1->vel - link2->vel) * frictionConstant;					//The friction force is added to the force to get the net force
		
			link1->applyForce(force);												//The net force is applied to link1
			link2->applyForce(-force);												//The opposite force is applied to link2
		}
	};
	
	class Strand
	{
	public:
		int numOfLinks;
		int numOfSprings;
	
		Link** links;
		Spring** springs;
	
		Vector3D gravitation;
		Vector3D connectionPos;
		
		float airFrictionConstant;
		
		Strand(int numOfLinks,
			   float mass,
			   float springConstant,
			   float springLength,
			   float springFrictionConstant,
			   Vector3D gravitation,
			   float airFrictionConstant,
			   Vector3D connectionPos=Vector3D())
		{
			this->numOfLinks = numOfLinks;
		
			this->gravitation = gravitation;					//Gravitational acceleration
			
			this-> connectionPos = connectionPos;				//Set the connection point of the strand
			
		 	this->airFrictionConstant = airFrictionConstant; 	//TODO: change to dampeningConstant
		
			links = new Link*[numOfLinks];						//Create array of Link pointers
			
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index] = new Link(mass);					//Create a Link as a pointer and store it in the array
				links[index]->pos.x = connectionPos.x + index * springLength;
				links[index]->pos.y = connectionPos.y;
				links[index]->pos.z = connectionPos.z;
			}
		
			numOfSprings = numOfLinks - 1;
		
			springs = new Spring*[numOfSprings];
		
			for(int index = 0; index < numOfSprings; index++)
			{
				springs[index] = new Spring(links[index], links[index+1],
											springConstant, springLength,
											springFrictionConstant);
			}
		}
		
		void release()
		{
			
			//Delete the springs
			for(int index = 0; index < numOfSprings; index++)
			{
				delete springs[index];
				springs[index] = NULL;
			}
			
			delete springs;
			springs = NULL;
			
			
			//Delete the links
			for(int index = 0; index < numOfLinks; index++)
			{
				delete links[index];
				links[index] = NULL;	
			}
		
			delete links;
			links = NULL;
		}
	
		Link* getLink(int index)
		{
			if(index < 0 || index >= numOfLinks)
				return NULL;
		
			return links[index];
		}
	
		void init()
		{
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->init();
			}
		}
	
		void solve()
		{
			for(int index = 0; index < numOfSprings; index++)
			{
				springs[index]->solve();
			}
		
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->applyForce(gravitation * links[index]->mass);			//The gravitational force
				links[index]->applyForce(-links[index]->vel * airFrictionConstant);	//The air friction
			}
		}
	
		void simulate(float dt)
		{
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->simulate(dt);
			}
			
			links[0]->pos = connectionPos;
		}
		
		void operate(float dt)
		{
			init();
			solve();
			simulate(dt);
		}
	};
	
	class HairSimulation
	{
	public:
		int numOfLinks;
		int numOfSprings;
	
		Link** links;
		Spring** springs;
	
		Vector3D gravitation;
		Vector3D connectionPos;
	
		float airFrictionConstant;
		
		HairSimulation(int numOfLinks,
					   float mass,
					   float springConstant,
					   float springLength,
					   float springFrictionConstant,
					   Vector3D gravitation,
					   float airFrictionConstant,
					   Vector3D connectionPos=Vector3D())
		{
			this->numOfLinks = numOfLinks;
		
			this->gravitation = gravitation;					//Gravitational acceleration
			
			this-> connectionPos = connectionPos;				//Set the connection point of the strand
			
			this->airFrictionConstant = airFrictionConstant; 	//TODO: change to dampeningConstant
		
			links = new Link*[numOfLinks];						//Create array of Link pointers
			
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index] = new Link(mass);					//Create a Link as a pointer and store it in the array
				links[index]->pos.x = connectionPos.x + index * springLength;
				links[index]->pos.y = connectionPos.y;
				links[index]->pos.z = connectionPos.z;
			}
			
			numOfSprings = numOfLinks - 1;
			
			springs = new Spring*[numOfSprings];
			
			for(int index = 0; index < numOfSprings; index++)
			{
				springs[index] = new Spring(links[index], links[index+1],
											springConstant, springLength,
											springFrictionConstant);
			}
		}
	
		void release()
		{
			for(int index = 0; index < numOfSprings; index++)
			{
				delete springs[index];
				springs[index] = NULL;
			}
			
			delete [] springs;
			springs = NULL;
			
			for(int index = 0; index < numOfLinks; index++)
			{
				delete links[index];
				links[index] = NULL;	
			}
		
			delete [] links;
			links = NULL;
		}
	
		Link* getLink(int index)
		{
			if(index < 0 || index >= numOfLinks)
				return NULL;
		
			return links[index];
		}
	
		void init()
		{
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->init();
			}
		}
	
		void solve()
		{
			for(int index = 0; index < numOfSprings; index++)
			{
				springs[index]->solve();
			}
		
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->applyForce(gravitation * links[index]->mass);			//The gravitational force
				links[index]->applyForce(-links[index]->vel * airFrictionConstant);	//The air friction
			}
		}
	
		void simulate(float dt)
		{
			for(int index = 0; index < numOfLinks; index++)
			{
				links[index]->simulate(dt);
			}
			
			links[0]->pos = connectionPos;
		}
	
		void operate(float dt)
		{
			init();
			solve();
			simulate(dt);
		}
	};
	
	class HairSimulationP
	{
	private:
		Strand** strands;
		
		
	public:
		int numStrands;
		HairSimulationP(int numOfLinks,
						float mass,
						float springConstant,
					 	float springLength,
					 	float springFrictionConstant,
					 	Vector3D gravitation,
					 	float airFrictionConstant,
					 	std::vector<Vector3D> &positions)
		{
			//Get the number of desired hair strands
			this->numStrands = positions.size();
			
			strands = new Strand*[numStrands];
			
			//Create a new bunch of strands at the given positions
			for(int index = 0; index < numStrands; index++)
			{
				Vector3D v(positions[index].x,positions[index].y,positions[index].z);
				
				strands[index] = new Strand(numOfLinks, mass, springConstant,
											springLength, springFrictionConstant,
											gravitation, airFrictionConstant,
											v);
			}
		}
		
		Strand* getStrand(int index)
		{
			if(index < 0 || index >= numStrands)
			{
				return NULL;
			}
			
			return strands[index];
		}
		
		void operate(float dt)
		{
			for(int index = 0; index < numStrands; index++)
			{
				strands[index]->operate(dt);
			}
		}
		
		void release()
		{
			for(int index = 0; index < numStrands; index++)
			{
				strands[index]->release();
				delete strands[index];
				strands[index] = NULL;
			}
			
			delete [] strands;
		}
	};
}
